#include "StdAfx.h"

#include "OperatorQueue.h"

namespace AnimPoseModifier {

/*
COperatorQueue
*/

CRYREGISTER_CLASS(COperatorQueue)

//

COperatorQueue::COperatorQueue()
{
	m_ops[0].reserve(8);
	m_ops[1].reserve(8);
	m_current = 0;
};

COperatorQueue::~COperatorQueue()
{
}

//

void COperatorQueue::PushPosition(uint32 jointIndex, EOp eOp, const Vec3& value)
{
	SOp op;
	op.joint = jointIndex;
	op.target = eTarget_Position;
	op.op = eOp;
	op.value[0] = value.x;
	op.value[1] = value.y;
	op.value[2] = value.z;
	op.value[3] = 0.0f;
	m_ops[m_current].push_back(op);
}

void COperatorQueue::PushOrientation(uint32 jointIndex, EOp eOp, const Quat& value)
{
	SOp op;
	op.joint = jointIndex;
	op.target = eTarget_Orientation;
	op.op = eOp;
	op.value[0] = value.v.x;
	op.value[1] = value.v.y;
	op.value[2] = value.v.z;
	op.value[3] = value.w;
	m_ops[m_current].push_back(op);
}

// IAnimationPoseModifier

bool COperatorQueue::Prepare(const SAnimationPoseModiferParams& params)
{
	++m_current &= 1;
	m_ops[m_current].clear();
	return true;
}

SPU_INDIRECT(CommandBufferExecute(ML))
bool COperatorQueue::Execute(const SAnimationPoseModiferParams& params)
{
	ICharacterModelSkeleton* pCharacterModelSkeleton = params.GetICharacterModelSkeleton();

	const std::vector<SOp>& ops = m_ops[(m_current+1)&1];
	uint32 opCount = uint32(ops.size());
	for (uint32 i=0; i<opCount; ++i)
	{
		const SOp& op = ops[i];
		uint32 jointIndex = op.joint;
		if (jointIndex >= params.jointCount)
			continue;

		switch (op.op)
		{
		case eOp_Override:
			{
			QuatT &output = params.pPoseAbsolute[jointIndex];

			switch (op.target)
			{
			case eTarget_Position:
				output.t.x = op.value[0];
				output.t.y = op.value[1];
				output.t.z = op.value[2];
				break;

			case eTarget_Orientation:
				output.q.v.x	= op.value[0];
				output.q.v.y	= op.value[1];
				output.q.v.z	= op.value[2];
				output.q.w		= op.value[3];
				break;
			}

			int32 parent = pCharacterModelSkeleton->GetJointParentIDByID(op.joint);
			if (parent < 0)
			{
				params.pPoseRelative[jointIndex] = output;
				continue;
			}

			params.pPoseRelative[jointIndex] = params.pPoseAbsolute[parent].GetInverted() * output;
			break;
		}

		case eOp_Additive:
			{
				QuatT &output = params.pPoseAbsolute[jointIndex];

				switch (op.target)
				{
				case eTarget_Position:
					output.t.x += op.value[0];
					output.t.y += op.value[1];
					output.t.z += op.value[2];
					break;

				case eTarget_Orientation:
					const Quat newRot(op.value[3], op.value[0], op.value[1], op.value[2]);
					output.q = newRot * output.q;
					break;
				}

				int32 parent = pCharacterModelSkeleton->GetJointParentIDByID(op.joint);
				if (parent < 0)
				{
					params.pPoseRelative[jointIndex] = output;
					continue;
				}

				params.pPoseRelative[jointIndex] = params.pPoseAbsolute[parent].GetInverted() * output;

			break;
			}
		}
	}

	return true;
}

void COperatorQueue::Synchronize()
{
}

} // namespace AnimPoseModifier
